{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Introduction to numerical robotics\n",
    "\n",
    "This notebook is a general introduction to Pinocchio. It shows how to manipulate the geometry model of a robot manipulator: set the configuration, compute the position of the end effector, check for collisions or the distance to an obstacle. The main idea is to give a brief introduction of the general topic: how to discover and learn a robot movement constrained by the environment, using iterative optimization methods.\n",
    "\n",
    "本笔记本是对Pinocchio的一般性介绍。它展示了如何操作机器人机械臂的几何模型：设置构型、计算末端执行器的位置、检查碰撞或与障碍物的距离。主要思想是对一个普遍主题进行简要介绍：如何使用迭代优化方法，发现并学习受环境约束的机器人运动。 \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "NB: as for all the tutorials, a magic command %do_not_load is introduced to hide the solutions to some questions. Change it for %load if you want to see (and execute) the solution.\n"
     ]
    }
   ],
   "source": [
    "import magic_donotload"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Set up\n",
    "\n",
    "Let us load the UR5 robot model, the Pinocchio library, some optimization functions from SciPy and the Matplotlib for plotting:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import pinocchio as pin\n",
    "from utils.meshcat_viewer_wrapper import MeshcatVisualizer\n",
    "import time\n",
    "import numpy as np\n",
    "from numpy.linalg import inv,norm,pinv,svd,eig\n",
    "from scipy.optimize import fmin_bfgs,fmin_slsqp\n",
    "from utils.load_ur5_with_obstacles import load_ur5_with_obstacles,Target\n",
    "import matplotlib.pylab as plt"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's first load the robot model and display it. For this tutorial, a single utility function will load the robot model and create obstacles around it:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = load_ur5_with_obstacles(reduced=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The next few lines initialize a 3D viewer."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7000/static/\n"
     ]
    }
   ],
   "source": [
    "viz = MeshcatVisualizer(robot)\n",
    "viz.display(robot.q0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "            <div style=\"height: 400px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
       "            <iframe src=\"http://127.0.0.1:7000/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "            </div>\n",
       "            "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "hasattr(viz.viewer, 'jupyter_cell') and viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The robot and the red obstacles are encoded in the `robot` object (we will not look in depth at what is inside this object). You can display a new configuration of the robot with `viz.display`. It takes a `numpy.array` of dimension 2 as input:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.display(np.array([3.,-1.5]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We also set up a target with is visualized as a green dot:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "target_pos = np.array([.5,.5])\n",
    "target = Target(viz,position = target_pos)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The `Target` object is the green dot that the robot should reach. You can change the target position by editing `target.position`, and display the new position with `target.display()`."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Using the robot model\n",
    "The robot is originally a 6 degrees-of-freedom (DOF) manipulator. Yet to make the example simple, we will only use its joints 1 and 2. The model has simply be loaded with \"frozen\" extra joints, which will then not appear in this notebook. Reload the model with `reduced=False` if you want to recover a model with full DOF."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following function computes the position of the end effector (in 2d):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "def endef(q):\n",
    "     '''Return the 2d position of the end effector.'''\n",
    "     pin.framesForwardKinematics(robot.model, robot.data, q)\n",
    "     return robot.data.oMf[-1].translation[[0, 2]]\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This function checks if the robot is in collision, and returns `True` if a collision is detected."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "def coll(q):\n",
    "     '''Return True if in collision, false otherwise.'''\n",
    "     pin.updateGeometryPlacements(robot.model, robot.data, robot.collision_model, robot.collision_data, q)\n",
    "     return pin.computeCollisions(robot.collision_model, robot.collision_data, False)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The next function computes the distance between the end effector and the target."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Your code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "def dist(q):\n",
    "     '''Return the distance between the end effector end the target (2d).'''\n",
    "     print(f\"q={q}\")\n",
    "     return 0.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Solution"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp0/generated/simple_path_planning_dist"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Random search of a valid configuration\n",
    "The free space is difficult to represent explicitely. We can sample the configuration space until a free configuration is found:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "def qrand(check=False):\n",
    "    '''Return a random configuration. If `check` is True, this configuration is not is collision.'''\n",
    "    pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The solution if needed:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp0/generated/simple_path_planning_qrand"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.display(qrand(check=True))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's now find a valid configuration that is arbitrarily close to the target: sample until dist is small enough and coll is false (you may want to display the random trials inside the loop)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## From a random configuration to the target\n",
    "Let' s now start from a random configuration. How can we find a path that bring the robot toward the target without touching the obstacles. Any idea?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Random descent: crawling from one free configuration to the target with random\n",
    "# steps.\n",
    "def randomDescent(q0 = None):\n",
    "     '''\n",
    "     Make a random walk of 0.1 step toward target\n",
    "     Return the list of configurations visited\n",
    "     '''\n",
    "     q = qrand(check=True) if q0 is None else q0\n",
    "     hist = [ q.copy() ]\n",
    "     # DO the walk\n",
    "     return hist"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Clue:\n",
    "- Use a random walk sampling random direction to extend\n",
    "- Bias the walk toward the goal by keeping the extension only if it is closer to the goal"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And solution if needed"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp0/generated/simple_path_planning_random_descent"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "ename": "AttributeError",
     "evalue": "'NoneType' object has no attribute 'copy'",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mAttributeError\u001b[0m                            Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[25], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m \u001b[43mrandomDescent\u001b[49m\u001b[43m(\u001b[49m\u001b[43m)\u001b[49m\n",
      "Cell \u001b[0;32mIn[15], line 9\u001b[0m, in \u001b[0;36mrandomDescent\u001b[0;34m(q0)\u001b[0m\n\u001b[1;32m      4\u001b[0m \u001b[38;5;250m\u001b[39m\u001b[38;5;124;03m'''\u001b[39;00m\n\u001b[1;32m      5\u001b[0m \u001b[38;5;124;03mMake a random walk of 0.1 step toward target\u001b[39;00m\n\u001b[1;32m      6\u001b[0m \u001b[38;5;124;03mReturn the list of configurations visited\u001b[39;00m\n\u001b[1;32m      7\u001b[0m \u001b[38;5;124;03m'''\u001b[39;00m\n\u001b[1;32m      8\u001b[0m q \u001b[38;5;241m=\u001b[39m qrand(check\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mTrue\u001b[39;00m) \u001b[38;5;28;01mif\u001b[39;00m q0 \u001b[38;5;129;01mis\u001b[39;00m \u001b[38;5;28;01mNone\u001b[39;00m \u001b[38;5;28;01melse\u001b[39;00m q0\n\u001b[0;32m----> 9\u001b[0m hist \u001b[38;5;241m=\u001b[39m [ \u001b[43mq\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mcopy\u001b[49m() ]\n\u001b[1;32m     10\u001b[0m \u001b[38;5;66;03m# DO the walk\u001b[39;00m\n\u001b[1;32m     11\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m hist\n",
      "\u001b[0;31mAttributeError\u001b[0m: 'NoneType' object has no attribute 'copy'"
     ]
    }
   ],
   "source": [
    "randomDescent()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Configuration space\n",
    "Let's try to have a better look of the configuration space. In this case, it is easy, as it is dimension 2: we can sample it exhaustively and plot it in 2D. For that, let's introduce another function to compute the distance to collision:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [],
   "source": [
    "def collisionDistance(q):\n",
    "     '''Return the minimal distance between robot and environment. '''\n",
    "     pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q)\n",
    "     if pin.computeCollisions(robot.collision_model,robot.collision_data,False):\n",
    "         return 0.0\n",
    "     idx = pin.computeDistances(robot.collision_model,robot.collision_data)\n",
    "     return robot.collision_data.distanceResults[idx].min_distance"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, let's sample the configuration space and plot the distance-to-target and the distance-to-obstacle field (I put 500 samples to spare your CPU, but you need at least 10x more for obtaining a good picture)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [],
   "source": [
    "def sampleSpace(nbSamples=500):\n",
    "     '''\n",
    "     Sample nbSamples configurations and store them in two lists depending\n",
    "     if the configuration is in free space (hfree) or in collision (hcol), along\n",
    "     with the distance to the target and the distance to the obstacles.\n",
    "     '''\n",
    "     hcol = []\n",
    "     hfree = []\n",
    "     for i in range(nbSamples):\n",
    "          q = qrand(False)\n",
    "          if not coll(q):\n",
    "               hfree.append( list(q.flat) + [ dist(q), collisionDistance(q) ])\n",
    "          else:\n",
    "               hcol.append(  list(q.flat) + [ dist(q), 1e-2 ])\n",
    "     return hcol,hfree\n",
    "\n",
    "def plotConfigurationSpace(hcol,hfree,markerSize=20):\n",
    "     '''\n",
    "     Plot 2 \"scatter\" plots: the first one plot the distance to the target for \n",
    "     each configuration, the second plots the distance to the obstacles (axis q1,q2, \n",
    "     distance in the color space).\n",
    "     '''\n",
    "     htotal = hcol + hfree\n",
    "     h=np.array(htotal)\n",
    "     plt.subplot(2,1,1)\n",
    "     plt.scatter(h[:,0],h[:,1],c=h[:,2],s=markerSize,lw=0)\n",
    "     plt.title(\"Distance to the target\")\n",
    "     plt.colorbar()\n",
    "     plt.subplot(2,1,2)\n",
    "     plt.scatter(h[:,0],h[:,1],c=h[:,3],s=markerSize,lw=0)\n",
    "     plt.title(\"Distance to the obstacles\")\n",
    "     plt.colorbar()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [
    {
     "ename": "ArgumentError",
     "evalue": "Python argument types in\n    pinocchio.pinocchio_pywrap.updateGeometryPlacements(Model, Data, GeometryModel, GeometryData, NoneType)\ndid not match C++ signature:\n    updateGeometryPlacements(pinocchio::ModelTpl<double, 0, pinocchio::JointCollectionDefaultTpl> model, pinocchio::DataTpl<double, 0, pinocchio::JointCollectionDefaultTpl> data, pinocchio::GeometryModel geometry_model, pinocchio::GeometryData {lvalue} geometry_data)\n    updateGeometryPlacements(pinocchio::ModelTpl<double, 0, pinocchio::JointCollectionDefaultTpl> model, pinocchio::DataTpl<double, 0, pinocchio::JointCollectionDefaultTpl> {lvalue} data, pinocchio::GeometryModel geometry_model, pinocchio::GeometryData {lvalue} geometry_data, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > q)",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mArgumentError\u001b[0m                             Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[28], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m hcol,hfree \u001b[38;5;241m=\u001b[39m \u001b[43msampleSpace\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;241;43m5000\u001b[39;49m\u001b[43m)\u001b[49m\n\u001b[1;32m      2\u001b[0m plotConfigurationSpace(hcol,hfree)\n",
      "Cell \u001b[0;32mIn[27], line 11\u001b[0m, in \u001b[0;36msampleSpace\u001b[0;34m(nbSamples)\u001b[0m\n\u001b[1;32m      9\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m i \u001b[38;5;129;01min\u001b[39;00m \u001b[38;5;28mrange\u001b[39m(nbSamples):\n\u001b[1;32m     10\u001b[0m      q \u001b[38;5;241m=\u001b[39m qrand(\u001b[38;5;28;01mFalse\u001b[39;00m)\n\u001b[0;32m---> 11\u001b[0m      \u001b[38;5;28;01mif\u001b[39;00m \u001b[38;5;129;01mnot\u001b[39;00m \u001b[43mcoll\u001b[49m\u001b[43m(\u001b[49m\u001b[43mq\u001b[49m\u001b[43m)\u001b[49m:\n\u001b[1;32m     12\u001b[0m           hfree\u001b[38;5;241m.\u001b[39mappend( \u001b[38;5;28mlist\u001b[39m(q\u001b[38;5;241m.\u001b[39mflat) \u001b[38;5;241m+\u001b[39m [ dist(q), collisionDistance(q) ])\n\u001b[1;32m     13\u001b[0m      \u001b[38;5;28;01melse\u001b[39;00m:\n",
      "Cell \u001b[0;32mIn[21], line 3\u001b[0m, in \u001b[0;36mcoll\u001b[0;34m(q)\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21mcoll\u001b[39m(q):\n\u001b[1;32m      2\u001b[0m \u001b[38;5;250m     \u001b[39m\u001b[38;5;124;03m'''Return True if in collision, false otherwise.'''\u001b[39;00m\n\u001b[0;32m----> 3\u001b[0m      \u001b[43mpin\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mupdateGeometryPlacements\u001b[49m\u001b[43m(\u001b[49m\u001b[43mrobot\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mmodel\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mrobot\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mdata\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mrobot\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mcollision_model\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mrobot\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mcollision_data\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mq\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m      4\u001b[0m      \u001b[38;5;28;01mreturn\u001b[39;00m pin\u001b[38;5;241m.\u001b[39mcomputeCollisions(robot\u001b[38;5;241m.\u001b[39mcollision_model, robot\u001b[38;5;241m.\u001b[39mcollision_data, \u001b[38;5;28;01mFalse\u001b[39;00m)\n",
      "\u001b[0;31mArgumentError\u001b[0m: Python argument types in\n    pinocchio.pinocchio_pywrap.updateGeometryPlacements(Model, Data, GeometryModel, GeometryData, NoneType)\ndid not match C++ signature:\n    updateGeometryPlacements(pinocchio::ModelTpl<double, 0, pinocchio::JointCollectionDefaultTpl> model, pinocchio::DataTpl<double, 0, pinocchio::JointCollectionDefaultTpl> data, pinocchio::GeometryModel geometry_model, pinocchio::GeometryData {lvalue} geometry_data)\n    updateGeometryPlacements(pinocchio::ModelTpl<double, 0, pinocchio::JointCollectionDefaultTpl> model, pinocchio::DataTpl<double, 0, pinocchio::JointCollectionDefaultTpl> {lvalue} data, pinocchio::GeometryModel geometry_model, pinocchio::GeometryData {lvalue} geometry_data, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > q)"
     ]
    }
   ],
   "source": [
    "hcol,hfree = sampleSpace(5000)\n",
    "plotConfigurationSpace(hcol,hfree)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can try to match your representation of the free space of the robot with this plot. \n",
    "As an example, you can display on this plot a feasible trajectory discover by random walk from an init position."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [],
   "source": [
    "traj = np.array([])\n",
    "qinit = np.array([-1.1, -3. ])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is a solution:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp0/generated/simple_path_planning_traj"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'hcol' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[31], line 2\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[38;5;66;03m# Add yourr traj to the plot, be carefull !\u001b[39;00m\n\u001b[0;32m----> 2\u001b[0m plotConfigurationSpace(\u001b[43mhcol\u001b[49m,hfree)\n\u001b[1;32m      3\u001b[0m plt\u001b[38;5;241m.\u001b[39mplot(traj[:,\u001b[38;5;241m0\u001b[39m],traj[:,\u001b[38;5;241m1\u001b[39m],\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mr\u001b[39m\u001b[38;5;124m'\u001b[39m,lw\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m3\u001b[39m)\n",
      "\u001b[0;31mNameError\u001b[0m: name 'hcol' is not defined"
     ]
    }
   ],
   "source": [
    "# Add yourr traj to the plot, be carefull !\n",
    "plotConfigurationSpace(hcol,hfree)\n",
    "plt.plot(traj[:,0],traj[:,1],'r',lw=3)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Optimize the distance under non-collision constraint\n",
    "Finally, let's use one of the optimizers from SciPy to search for a robot configuration that minimizes the distance to the target, under the constraint that the distance to collision is positive.\n",
    "For that, we define a *cost function* $cost: \\mathcal{C} \\to \\mathbb{R}$ (taking the robot configuration and returning a scalar) and a constraint function (taking again the robot configuration and returning a scalar that should be positive). We additionally use the \"callback\" functionnality of the solver to render the robot configuration corresponding to the current value of the decision variable inside the solver algorithm.\n",
    "We use the \"SLSQP\" solver from SciPy, which implements a \"sequential quadratic program\" algorithm and accepts constraints.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The constraint is a positive distance with a margin on $\\epsilon$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "epsilon = .01"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def cost(q):\n",
    "     pass\n",
    "     \n",
    "def constraint(q):\n",
    "     pass\n",
    "     \n",
    "def callback(q):\n",
    "     '''\n",
    "     At each optimization step, display the robot configuration in gepetto-viewer.\n",
    "     '''\n",
    "     viz.display(q)\n",
    "     time.sleep(.01)\n",
    "\n",
    "def optimize():\n",
    "     '''\n",
    "     Optimize from an initial random configuration to discover a collision-free\n",
    "     configuration as close as possible to the target.\n",
    "     USE fmin_slsqp, see doc online\n",
    "     '''"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is a valid solution:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp0/generated/simple_path_planning_optim"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Look at the output of the solver. It always returns a variable value, but sometimes the algorithm fails being traped in an unfeasible region. Most of the time, the solver converges to a local minimum where the final distance to the target is nonzero"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now you can write a planner that try to optimize and retry until a valid solition is found!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Your solution"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And the solution if you need it:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp0/generated/simple_path_planning_useit"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "robotics_course",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.17"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
